
import processing.serial.*;
Serial port; 
Serial port2;

// y - 66537
// x - 101695
// definitions -------------------------------------
int LINE_FEED = '\r'; 

// variables ---------------------------------------
int validData = 0;
int timeout;
int timeoutCount = 0;
String inBuffer;
int xpos, ypos; 
int FOV, FOV0, FOV1;
int tl_x, tl_y, br_x, br_y;
//int stepWidth, stepHeight;
float columns, rows;
int processStep = 0;
boolean advance = true;
int delay_before_pic = 2000;
int delay_after_pic = 3000;
int m = 0;
int n = 0;
String x_string = "0";
String y_string = "0";


//byte[] rspCharArray = new byte[32];

/////////////////// setup //////////////////////////
void setup(){
  size(1017,665);


  // List all the available serial ports:
  println(Serial.list());

  // I know that the first port in the serial list on my mac
  // is always my  Keyspan adaptor, so I open Serial.list()[0].
  // Open whatever port is the one you're using.
  port = new Serial(this, Serial.list()[0], 9600);
  port2 = new Serial(this, Serial.list()[2], 9600);
  delay(500);
  port.clear();
  //println("current position at startup: "+commandReceive("p\r"));

  xpos = 0;
  ypos = 0;
  FOV = 800;
  tl_x = 0;
  tl_y = 0;
  br_x = 0;
  br_y = 0;
  //stepWidth=0;
  //stepHeight=0;

  rectMode(CENTER);
  strokeCap(SQUARE);
  noFill();
  smooth();
}



///////////////////// draw ///////////////////////////
void draw(){
  background(100,100,100);
  translate(1017,665);
  rotate(PI);

  // Draw the shape
  rect(xpos, ypos, (FOV/200*4/3), (FOV/200));
  // draw box that encompasses the image space
  rect( (((tl_x - br_x)/2 + br_x)/100), (((tl_y - br_y)/2 + br_y)/100), (tl_x - br_x)/100, (tl_y - br_y)/100 );


  float stepWidth = 0.66 * float(FOV);
  float stepHeight = 0.75 * stepWidth;
  //println(stepWidth+", "+stepHeight);
  int arrayWidth = (tl_x - br_x);
  int arrayHeight = (tl_y - br_y);

  //println(commandReceive("py\r"));
  y_string = commandReceive("py\r");
  if (y_string != null){
    ypos = int(Float.parseFloat(y_string))/100;
  }
  x_string = commandReceive("px\r");
  if (x_string != null){
    xpos = int(Float.parseFloat(x_string))/100;
  }
  //ypos = int(Float.parseFloat(commandReceive("py\r")))/100;
  //xpos = int(Float.parseFloat(commandReceive("px\r")))/100;
  //println(xpos + ", " + ypos);


  // now we can tell the user how many images they're going to have to take
  if(processStep == 3){
    br_x = int(Float.parseFloat(commandReceive("px\r")));
    br_y = int(Float.parseFloat(commandReceive("py\r")));
    columns = (tl_x - br_x)/(stepWidth);
    rows = (tl_y - br_y)/(stepHeight);
    println("columns: "+(int)columns+", rows: "+(int)rows+", total pics: "+((int)columns*(int)rows));
  }

  // now we loop through the array and going to each voxel, taking a picture
  if(processStep == 4){
    //println("step 4");
    //delay(1000);


    // calculate the new points based on cell info etc.
    int set_x = tl_x - (m * (int)stepWidth);
    int set_y = tl_y - (n * (int)stepHeight);

    // write the new points
    port.write("gx "+(set_x)+'\r');
    port.write("gy "+(set_y)+'\r');    

    // take a picture and wait
    if(advance == true) delay(20000);
    delay(delay_before_pic);      // if we could replace delay with sudo interrupt, things would look nicer...
    port2.write("picture\r");
    delay(delay_before_pic);


    // update cell position m,n
    println("m: "+m+" "+(set_x/100)+", n: "+n+" "+(set_y/100));
    n++;
    if( (n)*(int)stepHeight > arrayHeight ){
      m++;
      n=0;
    }
    if( (m)*(int)stepWidth > arrayWidth ){
      processStep++;
      advance = true;
    } 


    rect( (set_x/100), (set_y/100), stepWidth/100, stepHeight/100);

    /*
   // traverse the columns 
     for( int m = 0; (m-1)*(int)stepWidth < arrayWidth; m++ ){
     println("");
     // traverse the rows
     for( int n = 0; (n-1)*(int)stepHeight < arrayHeight; n++ ){
     
     // calculate center point
     int set_x = tl_x - (m * (int)stepWidth);
     int set_y = tl_y - (n * (int)stepHeight);
     
     // send command to go to center point
     commandReceive("gx "+set_x+"\r");
     commandReceive("gy "+set_y+"\r");
     
     // wait before and after taking a picture
     delay(delay_before_pic);
     // commandReceive("picture\r");
     // delay(delay_after_pic);
     
     //rect( (set_x/100), (set_y/100), stepWidth, stepHeight);
     //rect( (set_x/100), (set_y/100), stepWidth/100, stepHeight/100);
     print("m: "+m+" "+(set_x/100)+", n: "+n+" "+(set_y/100)+"\t");
     
     }
     
     }
     */

  }


  /// prompt
  if(advance == true){
    switch(processStep) {
    case 0: 
      println("Let's set the field of view");
      println("Align object with top of screen and press mouse button");
      advance = false;
      break;
    case 1: 
      println("Align object with bottom of screen and press mouse button");
      advance = false;
      break;
    case 2: 
      println("Align to top most left and press mouse button");
      advance = false;
      break;
    case 3: 
      println("Align to bottom most right and press mouse button");
      advance = false;
      delay(1000);
      break;  
    case 4:
      println("Taking micro-gigapan!");
      advance = false;
      break;
    case 5:
      println("all done!");
      advance = false;
      break;
    }
  }

  /// user input from mouse
  if(mousePressed){
    switch(processStep) {
    case 0: 
      FOV0 = int(Float.parseFloat(commandReceive("py\r")));
      processStep++;
      advance = true;
      break;
    case 1: 
      FOV1 = int(Float.parseFloat(commandReceive("py\r")));
      println("your field of view is: "+(FOV0-FOV1));
      FOV = (FOV0-FOV1);
      processStep++;
      advance = true;
      break;
    case 2: 
      tl_x = int(Float.parseFloat(commandReceive("px\r")));
      tl_y = int(Float.parseFloat(commandReceive("py\r")));
      println("top left is: "+tl_x+", "+tl_y);
      processStep++;
      advance = true;
      break;
    case 3: 
      br_x = int(Float.parseFloat(commandReceive("px\r")));
      br_y = int(Float.parseFloat(commandReceive("py\r")));
      println("bottom right is: "+br_x+", "+br_y);
      processStep++;
      advance = true;
      break;      

    }  
  }

}






/////////////////// commandReceive() ///////////////////
public String commandReceive(String command) {
  // issue the command
  port.write(command);
  //delay(500);
  // get the response
  validData = 0;
  timeout=0;
  while(validData == 0){
    delay(100);
    timeout += 1;
    if(timeout > 4){
      println("timeout");
      validData = -1;
    }
  }
  if(validData != -1){    // we got a good response, now do something with it
    //println("inBuffer: "+inBuffer);
    return new String(inBuffer);
  }
  return null;
}



//////////////////// serialEvent ///////////////////
void serialEvent(Serial port) {
  inBuffer = port.readStringUntil(LINE_FEED);
  if (inBuffer != null) {
    validData = 1;
  }
}

/*
///////////////////// cross ////////////////////////
 void cross(int x, int y, int dim) {
 strokeWeight(5);
 line((x-(dim/2)), y, (x+(dim/2)), y);
 line(x, (y-(dim/2)), x, (y+(dim/2)));
 
 }
 *//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
